#include <ros/ros.h>
#include <pr2_controllers_msgs/JointTrajectoryAction.h>
#include <actionlib/client/simple_action_client.h>
#include "right_arm.h"
#include <algorithm>

//! Initialize the action client and wait for action server to come up
RightArm::RightArm() {
  ROS_INFO("RightArm()");
  // tell the action client that we want to spin a thread by default
  traj_client_ = new TrajClient("r_arm_controller/joint_trajectory_action", true);

  // wait for action server to come up
  while(!traj_client_->waitForServer(ros::Duration(5.0))) {
    ROS_INFO("Waiting for the joint_trajectory_action server");
  }
}

  //! Clean up the action client
  RightArm::~RightArm() {
  ROS_INFO("~RightArm()");
    delete traj_client_;
  }

  bool RightArm::waitForResult(float duration) {
    return traj_client_->waitForResult(ros::Duration(duration));
  }

  //! Sends the command to start a given trajectory
  void RightArm::startTrajectory(pr2_controllers_msgs::JointTrajectoryGoal goal)
  {
    // When to start the trajectory: 0s from now
    goal.trajectory.header.stamp = ros::Time::now() + ros::Duration(0.0);
    traj_client_->sendGoal(goal);
  }

  pr2_controllers_msgs::JointTrajectoryGoal RightArm::armDownTrajectory() {
    ROS_INFO("Begin armDownTrajectory()");
    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

    // We will have one waypoint in this goal trajectory
    goal.trajectory.points.resize(1);

    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = -1.3217288907134987;
    goal.trajectory.points[ind].positions[1] = 1.2048239008005679;
    goal.trajectory.points[ind].positions[2] = -1.8987382583942274;
    goal.trajectory.points[ind].positions[3] = -1.8958778215340997;
    goal.trajectory.points[ind].positions[4] = -20.566944359028934;
    goal.trajectory.points[ind].positions[5] = -1.2450301005929685;
    goal.trajectory.points[ind].positions[6] = 0.14071691219523608;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }

    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(2.0);
    //we are done; return the goal
    return goal;
  }

  pr2_controllers_msgs::JointTrajectoryGoal
      RightArm::moveInTrajectory(float headPan) {
    ROS_INFO("Begin moveInTrajectory()");
    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(4);

    // Pick the shoulder angle.
    float headPanGoal = std::max(-0.5f, std::min(0.5f, headPan));
    headPanGoal *= 2.0f;
    float shoulderAngle = 0.25f;
    if (headPanGoal > 0.5f) {
      // Head is panned left.
      float alpha = headPanGoal;
      float beta = 1 - alpha;
      shoulderAngle =
          (alpha * -0.25836861649279941) + (beta * -0.56379659483593492);
    } else {
      // Head is panned right.
      float alpha = -1 * headPanGoal;
      float beta = 1 - alpha;
      shoulderAngle =
          (alpha * -1.1978664043821621) + (beta * -0.56379659483593492);
    }

    // First trajectory point
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = shoulderAngle;
    goal.trajectory.points[ind].positions[1] = 0.27470272126708906;
    goal.trajectory.points[ind].positions[2] = -0.62407774358000168;
    goal.trajectory.points[ind].positions[3] = -1.0573658612007069;
    goal.trajectory.points[ind].positions[4] = -21.480639097413039;
    goal.trajectory.points[ind].positions[5] = -0.55354283345011457;
    goal.trajectory.points[ind].positions[6] = 1.799320455311874;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 1 second after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);

    // Second trajectory point
    // Positions
    ind += 1;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = shoulderAngle;
    goal.trajectory.points[ind].positions[1] = 0.29957362865979864;
    goal.trajectory.points[ind].positions[2] = -0.64091500034883975;
    goal.trajectory.points[ind].positions[3] = -1.0499825549201469;
    goal.trajectory.points[ind].positions[4] = -20.179137488112382;
    goal.trajectory.points[ind].positions[5] = -0.85170946289263494;
    goal.trajectory.points[ind].positions[6] = 0.71677501431386603;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.25);
    // Positions
    ind += 1;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = shoulderAngle;
    goal.trajectory.points[ind].positions[1] = 0.27470272126708906;
    goal.trajectory.points[ind].positions[2] = -0.62407774358000168;
    goal.trajectory.points[ind].positions[3] = -1.0573658612007069;
    goal.trajectory.points[ind].positions[4] = -21.480639097413039;
    goal.trajectory.points[ind].positions[5] = -0.55354283345011457;
    goal.trajectory.points[ind].positions[6] = 1.799320455311874;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.5);

    // Positions
    ind += 1;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = shoulderAngle;
    goal.trajectory.points[ind].positions[1] = 0.29957362865979864;
    goal.trajectory.points[ind].positions[2] = -0.64091500034883975;
    goal.trajectory.points[ind].positions[3] = -1.0499825549201469;
    goal.trajectory.points[ind].positions[4] = -20.179137488112382;
    goal.trajectory.points[ind].positions[5] = -0.85170946289263494;
    goal.trajectory.points[ind].positions[6] = 0.71677501431386603;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j)
    {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 2 seconds after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.75);
    //we are done; return the goal
    return goal;
  }

  pr2_controllers_msgs::JointTrajectoryGoal
      RightArm::moveLeftTrajectory(float headPan) {
    ROS_INFO("Begin moveLeftTrajectory()");
    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(5);

    // Pick the shoulder angle.
    float headPanGoal = std::max(-0.5f, std::min(0.5f, headPan));
    headPanGoal *= 2.0f;
    float shoulderAngle = 0.25f;
    if (headPanGoal > 0.5f) {
      // Head is panned left.
      float alpha = headPanGoal;
      float beta = 1 - alpha;
      shoulderAngle =
          (alpha * -0.5/*-0.25836861649279941*/) + (beta * -0.56379659483593492);
    } else {
      // Head is panned right.
      float alpha = -1 * headPanGoal;
      float beta = 1 - alpha;
      shoulderAngle =
          (alpha * -1.1978664043821621) + (beta * -0.56379659483593492);
    }

    // First trajectory point
    // Pointing straight forward
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = shoulderAngle;
    goal.trajectory.points[ind].positions[1] = 0.1976367463189308;
    goal.trajectory.points[ind].positions[2] = -0.68886109343343582;
    goal.trajectory.points[ind].positions[3] = -1.3101355232763465;
    goal.trajectory.points[ind].positions[4] = -19.722492583724655;
    goal.trajectory.points[ind].positions[5] = -1.5;
    goal.trajectory.points[ind].positions[6] = 1.65;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j) {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 1 second after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);

    // Loop the last two points (move back-and-forth)
    for (int i = 0; i < 2; ++i) {

      // Second trajectory point
      // Pointing Left
      // Positions
      ind += 1;
      goal.trajectory.points[ind].positions.resize(7);
      goal.trajectory.points[ind].positions[0] = shoulderAngle;
      goal.trajectory.points[ind].positions[1] = 0.1976367463189308;
      goal.trajectory.points[ind].positions[2] = -0.68886109343343582;
      goal.trajectory.points[ind].positions[3] = -1.3101355232763465;
      goal.trajectory.points[ind].positions[4] = -19.722492583724655;
      goal.trajectory.points[ind].positions[5] = -1.2238847654245655;
      goal.trajectory.points[ind].positions[6] = 0.75088600767194813;
      // Velocities
      goal.trajectory.points[ind].velocities.resize(7);
      for (size_t j = 0; j < 7; ++j) {
        goal.trajectory.points[ind].velocities[j] = 0.0;
      }
      // To be reached 2 seconds after starting along the trajectory
      goal.trajectory.points[ind].time_from_start =
          ros::Duration(1.0 + (ind * 0.5));

      // Third trajectory point
      // Pointing left, further left
      // Positions
      ind += 1;
      goal.trajectory.points[ind].positions.resize(7);
      goal.trajectory.points[ind].positions[0] = shoulderAngle;
      goal.trajectory.points[ind].positions[1] = 0.092908231515615947;
      goal.trajectory.points[ind].positions[2] = -1.0207955840191003;
      goal.trajectory.points[ind].positions[3] = -1.6273281519176555;
      goal.trajectory.points[ind].positions[4] = -19.615764708300873;
      goal.trajectory.points[ind].positions[5] = -0.74907192037769155;
      goal.trajectory.points[ind].positions[6] = 0.86144217364501707;
      // Velocities
      goal.trajectory.points[ind].velocities.resize(7);
      for (size_t j = 0; j < 7; ++j) {
        goal.trajectory.points[ind].velocities[j] = 0.0;
      }
      // To be reached 2 seconds after starting along the trajectory
      goal.trajectory.points[ind].time_from_start =
        ros::Duration(1.0 + (ind * 0.5));

    }

    //we are done; return the goal
    return goal;
  }

  pr2_controllers_msgs::JointTrajectoryGoal
      RightArm::moveRightTrajectory(float headPan) {
    ROS_INFO("Begin moveRightTrajectory()");
    //our goal variable
    pr2_controllers_msgs::JointTrajectoryGoal goal;

    // First, the joint names, which apply to all waypoints
    goal.trajectory.joint_names.push_back("r_shoulder_pan_joint");
    goal.trajectory.joint_names.push_back("r_shoulder_lift_joint");
    goal.trajectory.joint_names.push_back("r_upper_arm_roll_joint");
    goal.trajectory.joint_names.push_back("r_elbow_flex_joint");
    goal.trajectory.joint_names.push_back("r_forearm_roll_joint");
    goal.trajectory.joint_names.push_back("r_wrist_flex_joint");
    goal.trajectory.joint_names.push_back("r_wrist_roll_joint");

    // We will have two waypoints in this goal trajectory
    goal.trajectory.points.resize(5);

    // Pick the shoulder angle.
    float headPanGoal = std::max(-0.5f, std::min(0.5f, headPan));
    headPanGoal *= 2.0f;
    float shoulderAngle = 0.25f;
    if (headPanGoal > 0.5f) {
      // Head is panned left.
      float alpha = headPanGoal;
      float beta = 1 - alpha;
      shoulderAngle =
          (alpha * -0.25836861649279941) + (beta * -0.56379659483593492);
    } else {
      // Head is panned right.
      float alpha = -1 * headPanGoal;
      float beta = 1 - alpha;
      shoulderAngle =
          (alpha * -1.1978664043821621) + (beta * -0.56379659483593492);
    }

    // First trajectory point
    // Pointing straight forward
    // Positions
    int ind = 0;
    goal.trajectory.points[ind].positions.resize(7);
    goal.trajectory.points[ind].positions[0] = shoulderAngle;
    goal.trajectory.points[ind].positions[1] = 0.15237846245804759;
    goal.trajectory.points[ind].positions[2] = -0.69495457683549144;
    goal.trajectory.points[ind].positions[3] = -1.234275670511378;
    goal.trajectory.points[ind].positions[4] = -23.089655820942884;
    goal.trajectory.points[ind].positions[5] = -1.5;
    goal.trajectory.points[ind].positions[6] = 1.65;
    // Velocities
    goal.trajectory.points[ind].velocities.resize(7);
    for (size_t j = 0; j < 7; ++j) {
      goal.trajectory.points[ind].velocities[j] = 0.0;
    }
    // To be reached 1 second after starting along the trajectory
    goal.trajectory.points[ind].time_from_start = ros::Duration(1.0);

    // Loop the last two points (move back-and-forth)
    for (int i = 0; i < 2; ++i) {

      // Second trajectory point
      // Pointing Left
      // Positions
      ind += 1;
      goal.trajectory.points[ind].positions.resize(7);
      goal.trajectory.points[ind].positions[0] = shoulderAngle;
      goal.trajectory.points[ind].positions[1] = 0.15237846245804759;
      goal.trajectory.points[ind].positions[2] = -0.69495457683549144;
      goal.trajectory.points[ind].positions[3] = -1.234275670511378;
      goal.trajectory.points[ind].positions[4] = -23.089655820942884;
      goal.trajectory.points[ind].positions[5] = -1.9833764746830855;
      goal.trajectory.points[ind].positions[6] = 2.232016665702341;
      // Velocities
      goal.trajectory.points[ind].velocities.resize(7);
      for (size_t j = 0; j < 7; ++j) {
        goal.trajectory.points[ind].velocities[j] = 0.0;
      }
      // To be reached 2 seconds after starting along the trajectory
      goal.trajectory.points[ind].time_from_start =
          ros::Duration(1.0 + (ind * 0.5));

      // Third trajectory point
      // Pointing left, further left
      // Positions
      ind += 1;
      goal.trajectory.points[ind].positions.resize(7);
      goal.trajectory.points[ind].positions[0] = shoulderAngle;
      goal.trajectory.points[ind].positions[1] = 0.2389190687939047;
      goal.trajectory.points[ind].positions[2] = -0.14317361929671302;
      goal.trajectory.points[ind].positions[3] = -1.0127764820945804;
      goal.trajectory.points[ind].positions[4] = -23.247173438709261;
      goal.trajectory.points[ind].positions[5] = -1.3615904975397819;
      goal.trajectory.points[ind].positions[6] = 2.3822529668062398;
      // Velocities
      goal.trajectory.points[ind].velocities.resize(7);
      for (size_t j = 0; j < 7; ++j) {
        goal.trajectory.points[ind].velocities[j] = 0.0;
      }
      // To be reached 2 seconds after starting along the trajectory
      goal.trajectory.points[ind].time_from_start =
        ros::Duration(1.0 + (ind * 0.5));

    }

    //we are done; return the goal
    return goal;
  }

  //! Returns the current state of the action
  actionlib::SimpleClientGoalState RightArm::getState()
  {
    return traj_client_->getState();
  }

